#include "return_to_charging_station.h"

ReturnToChargingStation::ReturnToChargingStation(const std::string& name,
                                                 const BT::NodeConfig& config, rclcpp::Node* node)
    : BT::StatefulActionNode(name, config), node_(node), return_complete_(false)
{
}

BT::PortsList ReturnToChargingStation::providedPorts() { return {}; }

BT::NodeStatus ReturnToChargingStation::onStart()
{
    RCLCPP_INFO(node_->get_logger(), "开始返回充电站");
    return_complete_ = false;

    // 模拟返回过程
    timer_ = node_->create_wall_timer(std::chrono::seconds(2),
                                      [this]()
                                      {
                                          RCLCPP_INFO(node_->get_logger(), "已返回充电站");
                                          return_complete_ = true;
                                          timer_->cancel();
                                      });

    return BT::NodeStatus::RUNNING;
}

BT::NodeStatus ReturnToChargingStation::onRunning()
{
    return return_complete_ ? BT::NodeStatus::SUCCESS : BT::NodeStatus::RUNNING;
}

void ReturnToChargingStation::onHalted() { RCLCPP_WARN(node_->get_logger(), "返回任务被中断"); }